#include "zos_config.h"

#ifdef BOARD_USING_MPU6XXX

#include <zos.h>

#include <string.h>
#include <stdlib.h>


#include "mpu6xxx.h"
#include "mpu6xxx_reg.h"

#ifdef BOARD_USING_MPU6XXX_MAG
#include "ak8963_reg.h"
#endif 

#define MPU6XXX_ACCEL_SEN     (16384)
#define MPU6XXX_GYRO_SEN      (1310)

#define MPU60X0_SPI_MAX_SPEED (1000 * 1000)
#define MPU60X0_TEMP_SEN      (340)
#define MPU60X0_TEMP_OFFSET   (36.5)

#define MPU6500_TEMP_SEN      (333.87)
#define MPU6500_TEMP_OFFSET   (21)

// MAG
#define AK8963_RANGE          (4912)
#define AK8963_FULLSCALE      (32760)

/**
 * This function writes the value of the register for mpu6xxx
 *
 * @param dev the pointer of device driver structure
 * @param reg the register for mpu6xxx
 * @param data value to write
 *
 * @return the writing status, ZOS_EOK reprensents  writing the value of the register successfully.
 */
static zos_err_t mpu6xxx_write_reg(struct mpu6xxx_device *dev, zos_uint8_t reg, zos_uint8_t data)
{
    zos_uint8_t buf[2];
	buf[0]=reg;
	buf[1]=data;
	return zos_i2c_write(dev->bus,dev->i2c_addr,&buf,2);
}

/**
 * This function reads the value of registers for mpu6xxx
 *
 * @param dev the pointer of device driver structure
 * @param reg the register for mpu6xxx
 * @param len number of register
 * @param buf read data pointer
 *
 * @return the reading status, ZOS_EOK reprensents  reading the value of registers successfully.
 */
static zos_err_t mpu6xxx_read_regs(struct mpu6xxx_device *dev, zos_uint8_t reg, zos_uint8_t len, zos_uint8_t *buf)
{
	if(zos_i2c_write(dev->bus,dev->i2c_addr,&reg,1) == ZOS_EOK)
    {
        return zos_i2c_read(dev->bus,dev->i2c_addr,buf,len);
    }

    return ZOS_ERROR;
}

/**
 * This function writes a bit value of registers for mpu6xxx
 *
 * @param dev the pointer of device driver structure
 * @param reg the register for mpu6xxx
 * @param bit the position of the register
 * @param data value to write
 *
 * @return the writing status, ZOS_EOK reprensents  writing a bit value of registers successfully.
 */
static zos_err_t mpu6xxx_write_bit(struct mpu6xxx_device *dev, zos_uint8_t reg, zos_uint8_t bit, zos_uint8_t data)
{
    zos_uint8_t byte;
    zos_err_t res;

    res = mpu6xxx_read_regs(dev, reg, 1, &byte);
    if (res != ZOS_EOK)
    {
        return res;
    }

    byte = (data != 0) ? (byte | (1 << bit)) : (byte & ~(1 << bit));

    return mpu6xxx_write_reg(dev, reg, byte);
}

/**
 * This function reads a bit value of registers for mpu6xxx
 *
 * @param dev the pointer of device driver structure
 * @param reg the register for mpu6xxx
 * @param bit the position of the register
 * @param data read data pointer
 *
 * @return the reading status, ZOS_EOK reprensents  reading a bit value of registers successfully.
 */
static zos_err_t mpu6xxx_read_bit(struct mpu6xxx_device *dev, zos_uint8_t reg, zos_uint8_t bit, zos_uint8_t *data)
{
    zos_uint8_t byte;
    zos_err_t res;

    res = mpu6xxx_read_regs(dev, reg, 1, &byte);
    if (res != ZOS_EOK)
    {
        return res;
    }

    *data = byte & (1 << bit);

    return ZOS_EOK;
}

/**
 * This function writes multi-bit value of registers for mpu6xxx
 *
 * @param dev the pointer of device driver structure
 * @param reg the register for mpu6xxx
 * @param start_bit the start position of the register
 * @param len number of bits to write
 * @param data value to write
 *
 * @return the writing status, ZOS_EOK reprensents  writing multi-bit value of registers successfully.
 */
static zos_err_t mpu6xxx_write_bits(struct mpu6xxx_device *dev, zos_uint8_t reg, zos_uint8_t start_bit, zos_uint8_t len, zos_uint8_t data)
{
    zos_uint8_t byte, mask;
    zos_err_t res;

    res = mpu6xxx_read_regs(dev, reg, 1, &byte);
    if (res != ZOS_EOK)
    {
        return res;
    }

    mask = ((1 << len) - 1) << (start_bit - len + 1);
    data <<= (start_bit - len + 1); // shift data into correct position
    data &= mask; // zero all non-important bits in data
    byte &= ~(mask); // zero all important bits in existing byte
    byte |= data; // combine data with existing byte

    return mpu6xxx_write_reg(dev, reg, byte);
}

/**
 * This function reads multi-bit value of registers for mpu6xxx
 *
 * @param dev the pointer of device driver structure
 * @param reg the register for mpu6xxx
 * @param start_bit the start position of the register
 * @param len number of bits to write
 * @param data read data pointer
 *
 * @return the reading status, ZOS_EOK reprensents  reading multi-bit value of registers successfully.
 */
static zos_err_t mpu6xxx_read_bits(struct mpu6xxx_device *dev, zos_uint8_t reg, zos_uint8_t start_bit, zos_uint8_t len, zos_uint8_t *data)
{
    zos_uint8_t byte, mask;
    zos_err_t res;

    res = mpu6xxx_read_regs(dev, reg, 1, &byte);
    if (res != ZOS_EOK)
    {
        return res;
    }

    mask = ((1 << len) - 1) << (start_bit - len + 1);
    byte &= mask;
    byte >>= (start_bit - len + 1);
    *data = byte;

    return ZOS_EOK;
}

// MAG
#ifdef BOARD_USING_MPU6XXX_MAG

#define MAG_READ_DELAY_TIME         50

static void mpu92_mag_write_reg(struct mpu6xxx_device *dev, zos_uint8_t addr, zos_uint8_t data)
{
    zos_uint8_t  status = 0;
    rt_uint32_t timeout = MAG_READ_DELAY_TIME;

    mpu6xxx_write_reg(dev, MPU6XXX_RA_I2C_SLV4_ADDR, AK8963_I2C_ADDR);
    mpu6xxx_write_reg(dev, MPU6XXX_RA_I2C_SLV4_REG, addr);
    mpu6xxx_write_reg(dev, MPU6XXX_RA_I2C_SLV4_DO, data);
    mpu6xxx_write_reg(dev, MPU6XXX_RA_I2C_SLV4_CTRL, MPU6500_I2C_SLVx_EN);

    do 
    {
        mpu6xxx_read_regs(dev, MPU6XXX_RA_I2C_MST_STATUS, 1, &status);
        zos_task_delay(1);
    } while (((status & MPU6500_I2C_SLV4_DONE) == 0) && (timeout--));

}

#endif // BOARD_USING_MPU6XXX_MAG

/**
 * This function gets the raw data of the accelerometer
 *
 * @param dev the pointer of device driver structure
 * @param accel the pointer of 3axes structure for receive data
 *
 * @return the reading status, ZOS_EOK reprensents  reading the data successfully.
 */
static zos_err_t mpu6xxx_get_accel_raw(struct mpu6xxx_device *dev, struct mpu6xxx_3axes *accel)
{
    zos_uint8_t buffer[6];
    zos_err_t res;

    res = mpu6xxx_read_regs(dev, MPU6XXX_RA_ACCEL_XOUT_H, 6, buffer);
    if (res != ZOS_EOK)
    {
        return res;
    }

    accel->x = ((zos_uint16_t)buffer[0] << 8) + buffer[1];
    accel->y = ((zos_uint16_t)buffer[2] << 8) + buffer[3];
    accel->z = ((zos_uint16_t)buffer[4] << 8) + buffer[5];

    return ZOS_EOK;
}

/**
 * This function gets the raw data of the gyroscope
 *
 * @param dev the pointer of device driver structure
 * @param gyro the pointer of 3axes structure for receive data
 *
 * @return the reading status, ZOS_EOK reprensents  reading the data successfully.
 */
static zos_err_t mpu6xxx_get_gyro_raw(struct mpu6xxx_device *dev, struct mpu6xxx_3axes *gyro)
{
    zos_uint8_t buffer[6];
    zos_err_t res;

    res = mpu6xxx_read_regs(dev, MPU6XXX_RA_GYRO_XOUT_H, 6, buffer);
    if (res != ZOS_EOK)
    {
        return res;
    }

    gyro->x = ((zos_uint16_t)buffer[0] << 8) + buffer[1];
    gyro->y = ((zos_uint16_t)buffer[2] << 8) + buffer[3];
    gyro->z = ((zos_uint16_t)buffer[4] << 8) + buffer[5];

    return ZOS_EOK;
}

#ifdef BOARD_USING_MPU6XXX_MAG
/**
 * This function gets the raw data of the magnetometer
 *
 * @param dev the pointer of device driver structure
 * @param mag the pointer of 3axes structure for receive data
 *
 * @return the reading status, ZOS_EOK reprensents  reading the data successfully.
 */
static zos_err_t mpu6xxx_get_mag_raw(struct mpu6xxx_device *dev, struct mpu6xxx_3axes *mag)
{
    zos_uint8_t buffer[8];
    zos_err_t res;

    res = mpu6xxx_read_regs(dev, MPU6XXX_RA_EXT_SENS_DATA_00, 8, buffer);
    if (res != ZOS_EOK)
    {
        return res;
    }

    mag->x = ((zos_uint16_t)buffer[2] << 8) + buffer[1];
    mag->y = ((zos_uint16_t)buffer[4] << 8) + buffer[3];
    mag->z = ((zos_uint16_t)buffer[6] << 8) + buffer[5];

    return ZOS_EOK;
}
#endif

/**
 * This function gets the raw data of the temperature
 *
 * @param dev the pointer of device driver structure
 * @param temp read data pointer
 *
 * @return the reading status, ZOS_EOK reprensents  reading the data successfully.
 */
static zos_err_t mpu6xxx_get_temp_raw(struct mpu6xxx_device *dev, zos_int16_t *temp)
{
    zos_uint8_t buffer[2];
    zos_err_t res;

    res = mpu6xxx_read_regs(dev, MPU6XXX_RA_TEMP_OUT_H, 2, buffer);
    if (res != ZOS_EOK)
    {
        return res;
    }

    *temp = ((zos_uint16_t)buffer[0] << 8) + buffer[1];

    return ZOS_EOK;
}

/**
 * This function gets mpu6xxx parameters.
 *
 * @param dev the pointer of device driver structure
 * @param cmd Configuration item
 * @param param read data pointer
 *
 * @return the reading status, ZOS_EOK reprensents  reading the data successfully.
 */
static zos_err_t mpu6xxx_get_param(struct mpu6xxx_device *dev, enum mpu6xxx_cmd cmd, zos_uint16_t *param)
{
    zos_uint8_t data = 0;
    zos_err_t res = ZOS_EOK;

    ZOS_ASSERT(dev);

    switch (cmd)
    {
    case MPU6XXX_GYRO_RANGE:  /* Gyroscope full scale range */
        res = mpu6xxx_read_bits(dev, MPU6XXX_RA_GYRO_CONFIG, MPU6XXX_GCONFIG_FS_SEL_BIT, MPU6XXX_GCONFIG_FS_SEL_LENGTH, &data);
        *param = data;
        break;
    case MPU6XXX_ACCEL_RANGE: /* Accelerometer full scale range */
        res = mpu6xxx_read_bits(dev, MPU6XXX_RA_ACCEL_CONFIG, MPU6XXX_ACONFIG_AFS_SEL_BIT, MPU6XXX_ACONFIG_AFS_SEL_LENGTH, &data);
        *param = data;
        break;
    case MPU6XXX_DLPF_CONFIG: /* Digital Low Pass Filter */
        res = mpu6xxx_read_bits(dev, MPU6XXX_RA_CONFIG, MPU6XXX_CFG_DLPF_CFG_BIT, MPU6XXX_CFG_DLPF_CFG_LENGTH, &data);
        *param = data;
        break;
    case MPU6XXX_SAMPLE_RATE: /* Sample Rate */
        /* Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) */
        res = mpu6xxx_read_bits(dev, MPU6XXX_RA_CONFIG, MPU6XXX_CFG_DLPF_CFG_BIT, MPU6XXX_CFG_DLPF_CFG_LENGTH, &data);
        if (res != ZOS_EOK)
        {
            break;
        }

        if (data == 0 || data == 7) /* dlpf is disable */
        {
            res = mpu6xxx_read_regs(dev, MPU6XXX_RA_SMPLRT_DIV, 1, &data);
            *param = 8000 / (data + 1);
        }
        else /* dlpf is enable */
        {
            res = mpu6xxx_read_regs(dev, MPU6XXX_RA_SMPLRT_DIV, 1, &data);
            *param = 1000 / (data + 1);
        }
        break;
    case MPU6XXX_SLEEP: /* sleep mode */
        res = mpu6xxx_read_bit(dev, MPU6XXX_RA_PWR_MGMT_1, MPU6XXX_PWR1_SLEEP_BIT, &data);
        *param = data;
        break;
    }

    return res;
}

/**
 * This function set mpu6xxx parameters.
 *
 * @param dev the pointer of device driver structure
 * @param cmd Configuration item
 * @param param Configuration item parameter
 *
 * @return the setting status, ZOS_EOK reprensents  setting the parameter successfully.
 */
zos_err_t mpu6xxx_set_param(struct mpu6xxx_device *dev, enum mpu6xxx_cmd cmd, zos_uint16_t param)
{
    zos_uint8_t data = 0;
    zos_err_t res = ZOS_EOK;

    ZOS_ASSERT(dev);

    switch (cmd)
    {
    case MPU6XXX_GYRO_RANGE:  /* Gyroscope full scale range */
        res = mpu6xxx_write_bits(dev, MPU6XXX_RA_GYRO_CONFIG, MPU6XXX_GCONFIG_FS_SEL_BIT, MPU6XXX_GCONFIG_FS_SEL_LENGTH, param);
        dev->config.gyro_range = param;
        break;
    case MPU6XXX_ACCEL_RANGE: /* Accelerometer full scale range */
        res = mpu6xxx_write_bits(dev, MPU6XXX_RA_ACCEL_CONFIG, MPU6XXX_ACONFIG_AFS_SEL_BIT, MPU6XXX_ACONFIG_AFS_SEL_LENGTH, param);
        dev->config.accel_range = param;
        break;
    case MPU6XXX_DLPF_CONFIG: /* Digital Low Pass Filter */
        res = mpu6xxx_write_bits(dev, MPU6XXX_RA_CONFIG, MPU6XXX_CFG_DLPF_CFG_BIT, MPU6XXX_CFG_DLPF_CFG_LENGTH, param);
        break;
    case MPU6XXX_SAMPLE_RATE: /* Sample Rate —— 16-bit unsigned value.
                                 Sample Rate = [1000 -  4]HZ when dlpf is enable
                                 Sample Rate = [8000 - 32]HZ when dlpf is disable */

        //Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
        res = mpu6xxx_read_bits(dev, MPU6XXX_RA_CONFIG, MPU6XXX_CFG_DLPF_CFG_BIT, MPU6XXX_CFG_DLPF_CFG_LENGTH, &data);
        if (res != ZOS_EOK)
        {
            break;
        }

        if (data == 0 || data == 7) /* dlpf is disable */
        {
            if (param > 8000)
                data = 0;
            else if (param < 32)
                data = 0xFF;
            else
                data = 8000 / param - 1;
        }
        else /* dlpf is enable */
        {
            if (param > 1000)
                data = 0;
            else if (param < 4)
                data = 0xFF;
            else
                data = 1000 / param - 1;
        }
        res = mpu6xxx_write_reg(dev, MPU6XXX_RA_SMPLRT_DIV, data);
        break;
    case MPU6XXX_SLEEP: /* Configure sleep mode */
        res = mpu6xxx_write_bit(dev, MPU6XXX_RA_PWR_MGMT_1, MPU6XXX_PWR1_SLEEP_BIT, param);
        break;
    }

    return res;
}

/**
 * This function gets the data of the accelerometer, unit: mg
 *
 * @param dev the pointer of device driver structure
 * @param accel the pointer of 3axes structure for receive data
 *
 * @return the reading status, ZOS_EOK reprensents  reading the data successfully.
 */
zos_err_t mpu6xxx_get_accel(struct mpu6xxx_device *dev, struct mpu6xxx_3axes *accel)
{
    struct mpu6xxx_3axes tmp;
    zos_uint16_t sen;
    zos_err_t res;

    res = mpu6xxx_get_accel_raw(dev, &tmp);
    if (res != ZOS_EOK)
    {
        return res;
    }

    sen = MPU6XXX_ACCEL_SEN >> dev->config.accel_range;

    accel->x = (zos_int32_t)tmp.x * 1000 / sen;
    accel->y = (zos_int32_t)tmp.y * 1000 / sen;
    accel->z = (zos_int32_t)tmp.z * 1000 / sen;

    return ZOS_EOK;
}

/**
 * This function gets the data of the gyroscope, unit: deg/10s
 * Here deg/10s means 10 times higher precision than deg/s. 
 *
 * @param dev the pointer of device driver structure
 * @param gyro the pointer of 3axes structure for receive data
 *
 * @return the reading status, ZOS_EOK reprensents  reading the data successfully.
 */
zos_err_t mpu6xxx_get_gyro(struct mpu6xxx_device *dev, struct mpu6xxx_3axes *gyro)
{
    struct mpu6xxx_3axes tmp;
    zos_uint16_t sen;
    zos_err_t res;

    res = mpu6xxx_get_gyro_raw(dev, &tmp);
    if (res != ZOS_EOK)
    {
        return res;
    }

    sen = MPU6XXX_GYRO_SEN >> dev->config.gyro_range;

    gyro->x = (zos_int32_t)tmp.x * 100 / sen;
    gyro->y = (zos_int32_t)tmp.y * 100 / sen;
    gyro->z = (zos_int32_t)tmp.z * 100 / sen;

    return ZOS_EOK;
}

#ifdef BOARD_USING_MPU6XXX_MAG

/**
 * This function gets the data of the magnetometer, unit: uT
 *
 * @param dev the pointer of device driver structure
 * @param gyro the pointer of 3axes structure for receive data
 *
 * @return the reading status, ZOS_EOK reprensents  reading the data successfully.
 */
zos_err_t mpu6xxx_get_mag(struct mpu6xxx_device *dev, struct mpu6xxx_3axes *mag)
{
    struct mpu6xxx_3axes tmp;
    zos_err_t res;

    res = mpu6xxx_get_mag_raw(dev, &tmp);
    if (res != ZOS_EOK)
    {
        return res;
    }

    mag->x = ((zos_int32_t)tmp.x * AK8963_RANGE) / AK8963_FULLSCALE;
    mag->y = ((zos_int32_t)tmp.y * AK8963_RANGE) / AK8963_FULLSCALE;
    mag->z = ((zos_int32_t)tmp.z * AK8963_RANGE) / AK8963_FULLSCALE;

    return ZOS_EOK;
}

#endif

/**
 * This function gets the data of the temperature, unit: Centigrade
 *
 * @param dev the pointer of device driver structure
 * @param temp read data pointer
 *
 * @return the reading status, ZOS_EOK reprensents  reading the data successfully.
 */
zos_err_t mpu6xxx_get_temp(struct mpu6xxx_device *dev, float *temp)
{
    zos_int16_t tmp;
    zos_err_t res;

    res = mpu6xxx_get_temp_raw(dev, &tmp);
    if (res != ZOS_EOK)
    {
        return res;
    }

    if (dev->id == MPU6050_WHO_AM_I)
    {
        /* mpu60x0: Temperature in degrees C = (TEMP_OUT Register Value as a signed quantity)/340 + 36.53 */
        *temp = (double)tmp / MPU60X0_TEMP_SEN + MPU60X0_TEMP_OFFSET;
    }
    else
    {
        /* mpu6500:  ((TEMP_OUT – RoomTemp_Offset)/Temp_Sensitivity)+ 21degC */
        *temp = (double)tmp / MPU6500_TEMP_SEN + MPU6500_TEMP_OFFSET;
    }

    return ZOS_EOK;
}

/**
* This function sets the offset of the accelerometer
 *
 * @param dev the pointer of device driver structure
 * @param offset the pointer of 3axes structure of offsets
 *
 * @return the setting status, ZOS_EOK reprensents  setting the offsets successfully.
 */
zos_err_t mpu6xxx_set_accel_offset(struct mpu6xxx_device *dev, struct mpu6xxx_3axes *offset)
{
    zos_err_t res=0;
    res |= mpu6xxx_write_reg(dev, MPU6XXX_RA_XA_OFFS_H, (offset->x)>>8);
    res |= mpu6xxx_write_reg(dev, MPU6XXX_RA_XA_OFFS_L_TC, (offset->x)&0x00ff);
    res |= mpu6xxx_write_reg(dev, MPU6XXX_RA_YA_OFFS_H, (offset->y)>>8);
    res |= mpu6xxx_write_reg(dev, MPU6XXX_RA_YA_OFFS_L_TC, (offset->y)&0x00ff);
    res |= mpu6xxx_write_reg(dev, MPU6XXX_RA_ZA_OFFS_H, (offset->z)>>8);
    res |= mpu6xxx_write_reg(dev, MPU6XXX_RA_ZA_OFFS_L_TC, (offset->z)&0x00ff);
    return res;
}

/**
* This function gets the offset of the accelerometer
 *
 * @param dev the pointer of device driver structure
 * @param offset the pointer of 3axes structure of offsets
 *
 * @return the setting status, ZOS_EOK reprensents  reading the offsets successfully.
 */
zos_err_t mpu6xxx_get_accel_offset(struct mpu6xxx_device *dev, struct mpu6xxx_3axes *offset)
{
    zos_uint8_t buffer[6];
    zos_err_t res;

    res = mpu6xxx_read_regs(dev, MPU6XXX_RA_XA_OFFS_H, 6, buffer);
    if (res != ZOS_EOK)
    {
        return res;
    }

    offset->x = ((zos_uint16_t)buffer[0] << 8) + buffer[1];
    offset->y = ((zos_uint16_t)buffer[2] << 8) + buffer[3];
    offset->z = ((zos_uint16_t)buffer[4] << 8) + buffer[5];

return ZOS_EOK;
}

/**
* This function sets the offset of the gyroscope
 *
 * @param dev the pointer of device driver structure
 * @param offset the pointer of 3axes structure of offsets
 *
 * @return the setting status, ZOS_EOK reprensents  setting the offsets successfully.
 */
zos_err_t mpu6xxx_set_gyro_offset(struct mpu6xxx_device *dev, struct mpu6xxx_3axes *offset)
{
    zos_err_t res=0;
    res |= mpu6xxx_write_reg(dev, MPU6XXX_RA_XG_OFFS_USRH, (offset->x)>>8);
    res |= mpu6xxx_write_reg(dev, MPU6XXX_RA_XG_OFFS_USRL, (offset->x)&0x00ff);
    res |= mpu6xxx_write_reg(dev, MPU6XXX_RA_YG_OFFS_USRH, (offset->y)>>8);
    res |= mpu6xxx_write_reg(dev, MPU6XXX_RA_YG_OFFS_USRL, (offset->y)&0x00ff);
    res |= mpu6xxx_write_reg(dev, MPU6XXX_RA_ZG_OFFS_USRH, (offset->z)>>8);
    res |= mpu6xxx_write_reg(dev, MPU6XXX_RA_ZG_OFFS_USRL, (offset->z)&0x00ff);
    return res;
}

/**
* This function gets the offset of the gyroscope
 *
 * @param dev the pointer of device driver structure
 * @param offset the pointer of 3axes structure of offsets
 *
 * @return the setting status, ZOS_EOK reprensents  reading the offsets successfully.
 */
zos_err_t mpu6xxx_get_gyro_offset(struct mpu6xxx_device *dev, struct mpu6xxx_3axes *offset)
{
    zos_uint8_t buffer[6];
    zos_err_t res;

    res = mpu6xxx_read_regs(dev, MPU6XXX_RA_XG_OFFS_USRH, 6, buffer);
    if (res != ZOS_EOK)
    {
        return res;
    }

    offset->x = ((zos_uint16_t)buffer[0] << 8) + buffer[1];
    offset->y = ((zos_uint16_t)buffer[2] << 8) + buffer[3];
    offset->z = ((zos_uint16_t)buffer[4] << 8) + buffer[5];

return ZOS_EOK;
}

/**
 * This function initialize the mpu6xxx device.
 *
 * @param dev_name the name of transfer device
 * @param param the i2c device address for i2c communication, ZOS_NULL for spi
 *
 * @return the pointer of device driver structure, ZOS_NULL reprensents  initialization failed.
 */
struct mpu6xxx_device *mpu6xxx_init(ZOS_I2C_BUS bus, zos_uint8_t param)
{
    struct mpu6xxx_device *dev = ZOS_NULL;
    zos_uint8_t reg = 0xFF, res = ZOS_EOK;
    ZOS_I2C_CONFIG i2c_cfg;

    dev = zos_calloc(1, sizeof(struct mpu6xxx_device));
    if (dev == ZOS_NULL)
    {
        zos_printf("Can't allocate memory for mpu6xxx device on I2C ");
        goto __exit;
    }
    dev->bus=bus;
    i2c_cfg.mode=ZOS_I2C_MODE_MASTER;
    i2c_cfg.speed=ZOS_I2C_SPEED_100K;
    if(zos_i2c_init(bus,&i2c_cfg) == ZOS_EOK)
    {
        if (param != 0)
        {
            dev->i2c_addr = param;
        }
        else
        {
            /* find mpu6xxx device at address: 0x68 */
            dev->i2c_addr = MPU6XXX_ADDRESS_AD0_LOW;
            if (mpu6xxx_read_regs(dev, MPU6XXX_RA_WHO_AM_I, 1, &reg) != ZOS_EOK)
            {
                /* find mpu6xxx device at address 0x69 */
                dev->i2c_addr = MPU6XXX_ADDRESS_AD0_HIGH;
                if (mpu6xxx_read_regs(dev, MPU6XXX_RA_WHO_AM_I, 1, &reg) != ZOS_EOK)
                {
                    zos_printf("Can't find device at i2c!");
                    goto __exit;
                }
            }
            zos_printf("Device i2c address is:'0x%x'!", dev->i2c_addr);
        }
    }
    else
    {
        zos_printf("Unsupported device: i2c !");
        goto __exit;
    }

    if (mpu6xxx_read_regs(dev, MPU6XXX_RA_WHO_AM_I, 1, &reg) != ZOS_EOK)
    {
        zos_printf("Failed to read device id!");
        goto __exit;
    }

    dev->id = reg;

    switch (dev->id)
    {
    case MPU6050_WHO_AM_I:
        zos_printf("Find device: mpu6050!");
        break;
    case MPU6500_WHO_AM_I:
        zos_printf("Find device: mpu6500!");
        break;
    case MPU9250_WHO_AM_I:
        zos_printf("Find device: mpu9250!");
        break;
    case ICM20608G_WHO_AM_I:
    case ICM20608D_WHO_AM_I:
        zos_printf("Find device: icm20608!");
        break;
    case 0xFF:
        zos_printf("No device connection!");
        goto __exit;
    default:
        zos_printf("Unknown device id: 0x%x!", reg);
    }

    res += mpu6xxx_get_param(dev, MPU6XXX_ACCEL_RANGE, &dev->config.accel_range);
    res += mpu6xxx_get_param(dev, MPU6XXX_GYRO_RANGE, &dev->config.gyro_range);

    res += mpu6xxx_write_bits(dev, MPU6XXX_RA_PWR_MGMT_1, MPU6XXX_PWR1_CLKSEL_BIT, MPU6XXX_PWR1_CLKSEL_LENGTH, MPU6XXX_CLOCK_PLL_XGYRO);
    res += mpu6xxx_set_param(dev, MPU6XXX_GYRO_RANGE, MPU6XXX_GYRO_RANGE_250DPS);
    res += mpu6xxx_set_param(dev, MPU6XXX_ACCEL_RANGE, MPU6XXX_ACCEL_RANGE_2G);
    res += mpu6xxx_set_param(dev, MPU6XXX_SLEEP, MPU6XXX_SLEEP_DISABLE);

#ifdef BOARD_USING_MPU6XXX_MAG
    mpu6xxx_write_reg(dev, MPU6XXX_RA_USER_CTRL, 0x20);
    mpu92_mag_write_reg(dev, AK8963_REG_CNTL2, 0x01);      /* [0]  Reset Device                  */
    zos_task_delay(1);
    mpu92_mag_write_reg(dev, AK8963_REG_CNTL1, 0x00);      /* [1]  Power-down mode               */
    mpu92_mag_write_reg(dev, AK8963_REG_CNTL1, 0x0F);      /* [2]  Fuse ROM access mode          */
    mpu92_mag_write_reg(dev, AK8963_REG_CNTL1, 0x00);      /* [3]  Power-down mode               */
    zos_task_delay(1);    // 100us
    mpu92_mag_write_reg(dev, AK8963_REG_CNTL1, 0x16);      /* [4]  16bits and Continuous measurement mode 2 */

    /* config mpu9250 i2c */
    zos_task_delay(2);
    mpu6xxx_write_reg(dev, MPU6XXX_RA_I2C_MST_CTRL, 0x5D);
    zos_task_delay(2);
    mpu6xxx_write_reg(dev, MPU6XXX_RA_I2C_SLV0_ADDR, AK8963_I2C_ADDR | 0x80);
    zos_task_delay(2);
    mpu6xxx_write_reg(dev, MPU6XXX_RA_I2C_SLV0_REG, AK8963_REG_ST1);
    zos_task_delay(2);
    mpu6xxx_write_reg(dev, MPU6XXX_RA_I2C_SLV0_CTRL, MPU6500_I2C_SLVx_EN | 8);
    zos_task_delay(2);
    mpu6xxx_write_reg(dev, MPU6XXX_RA_I2C_SLV4_CTRL, 0x09);
    zos_task_delay(2);
    mpu6xxx_write_reg(dev, MPU6XXX_RA_I2C_MST_DELAY_CTRL, 0x81);
#endif

    if (res == ZOS_EOK)
    {
        zos_printf("Device init succeed!");
    }
    else
    {
        zos_printf("Error in device initialization!");
    }
    return dev;

__exit:
    if (dev != ZOS_NULL)
    {
        zos_free(dev);
    }
    return ZOS_NULL;
}

/**
 * This function releases memory
 *
 * @param dev the pointer of device driver structure
 */
void mpu6xxx_deinit(struct mpu6xxx_device *dev)
{
    ZOS_ASSERT(dev);

    zos_free(dev);
}

static void mpu6xxx(int argc, char **argv)
{
    static struct mpu6xxx_device *dev = ZOS_NULL;

    /* If the number of arguments less than 2 */
    if (argc < 2)
    {
        zos_printf("\n");
        zos_printf("mpu6xxx [OPTION] [PARAM]\n");
        zos_printf("         probe <dev_name>      Probe mpu6xxx by given name\n");
        zos_printf("         sr <var>              Set sample rate to var\n");
        zos_printf("                               var = [1000 -  4] when dlpf is enable\n");
        zos_printf("                               var = [8000 - 32] when dlpf is disable\n");
        zos_printf("         gr <var>              Set gyro range to var\n");
        zos_printf("                               var = [0 - 3] means [250 - 2000DPS]\n");
        zos_printf("         ar <var>              Set accel range to var\n");
        zos_printf("                               var = [0 - 3] means [2 - 16G]\n");
        zos_printf("         sleep <var>           Set sleep status\n");
        zos_printf("                               var = 0 means disable, = 1 means enable\n");
        zos_printf("         read [num]            read [num] times mpu6xxx\n");
        zos_printf("                               num default 5\n");
        return ;
    }
    else if (!strcmp(argv[1], "read"))
    {
        struct mpu6xxx_3axes accel, gyro;
        float temp;
        uint16_t num = 5;

        if (dev == ZOS_NULL)
        {
            zos_printf("Please probe mpu6xxx first!\n");
            return ;
        }
        if (argc == 3)
        {
            num = atoi(argv[2]);
        }

        while (num --)
        {
            mpu6xxx_get_accel(dev, &accel);
            mpu6xxx_get_gyro(dev, &gyro);
            mpu6xxx_get_temp(dev, &temp);

            zos_printf("accel.x = %4d, accel.y = %4d, accel.z = %4d ", accel.x, accel.y, accel.z);
            zos_printf("gyro.x = %4d gyro.y = %4d, gyro.z = %4d, ", gyro.x, gyro.y, gyro.z);
            zos_printf("temp = %d.%d\n", (int)(temp * 100) / 100, (int)(temp * 100) % 100);

            zos_task_delay(100);
        }
    }
    else if (argc == 3)
    {
        if (!strcmp(argv[1], "probe"))
        {
            if (dev)
            {
                mpu6xxx_deinit(dev);
            }
            dev = mpu6xxx_init(ZOS_I2C_BUS_0, 0);
        }
        else if (dev == ZOS_NULL)
        {
            zos_printf("Please probe mpu6xxx first!\n");
            return ;
        }
        else if (!strcmp(argv[1], "sr"))
        {
            mpu6xxx_set_param(dev, MPU6XXX_SAMPLE_RATE, atoi(argv[2]));
        }
        else if (!strcmp(argv[1], "sleep"))
        {
            mpu6xxx_set_param(dev, MPU6XXX_SLEEP, atoi(argv[2]));
        }
        else if (!strcmp(argv[1], "gr"))
        {
            mpu6xxx_set_param(dev, MPU6XXX_GYRO_RANGE, atoi(argv[2]));
        }
        else if (!strcmp(argv[1], "ar"))
        {
            mpu6xxx_set_param(dev, MPU6XXX_ACCEL_RANGE, atoi(argv[2]));
        }
        else
        {
            zos_printf("Unknown command, please enter 'mpu6xxx' get help information!\n");
        }
    }
    else
    {
        zos_printf("Unknown command, please enter 'mpu6xxx' get help information!\n");
    }
}

#include "shell.h"

SHELL_EXPORT_CMD(mpu6xxx,mpu6xxx, mpu6xxx demo);


#endif
